Coordinate-invariant algorithms for robot dynamics

In this article, we present, using methods from the theory of Lie groups and Lie algebras, a coordinate-invariant formulation of the dynamics of open kinematic chains. We first reformulate the recursive dynamics algorithm originally given in [8] for open chains in terms of standard linear operators on the Lie algebra of the Special Euclidean group. Using straight forward algebraic manipulations, we then recast the resulting algorithm into a set of closed-form dynamic equations; this transformation allows one to move easily between O(n) recursive algorithms advantageous for computation, and closed-form equations advantageous for symbolic manipulation and analysis. The transformation also illuminates how the choice of link reference frames affects the computational structure. We then reformulate Featherstone’s articulated body inertia algorithm [3] using this same geometric framework, and rederive Rodriguez et al.’s [11]–[13] square factorization of the mass matrix and its inverse. An efficient O(n) recursive algorithm for forward dynamics is also extracted from the inverse factorization. The resulting equations lead to a succinct high-level description of robot dynamics in both joint and operational space coordinates that minimizes symbolic complexity without sacrificing computational efficiency, and provides the basis for a dynamics formulation that does not require link reference frames in the description of the forward kinematics.